#include "histogramOfOpticFlow.h"
/*
  构造函数
 */
HistogramOfFlow::HistogramOfFlow(const char* _videoFileName, const char* _stipFileName, const char* _featuresVectorFileName):videoFileName(_videoFileName),stipFileName(_stipFileName),featuresVectorFileName(_featuresVectorFileName),PI(3.1415926),gradientThreshold(10)
{
  winSize = cvSize(32,32) ;
  blockSize = cvSize(16,16) ;
  init() ;
}
/*
  析构函数
  释放图像内存
 */
HistogramOfFlow::~HistogramOfFlow()
{
  while( !imageList.empty() )
    {
      IplImage * ptr = imageList.back().second ;
      cvReleaseImage(&ptr) ;
      imageList.pop_back() ;
    }
  
}
void HistogramOfFlow::init()
{
  // 进行block与cell的数据一致性 
  if( winSize.width % blockSize.width || winSize.height % blockSize.height ) 
    {
      ERROR("cellSize is not suit for blockSize!") ;
      exit(-1) ;
    }
  CvSize blockCells = cvSize(winSize.width/blockSize.width,winSize.height/blockSize.width) ;

  int nbins = 5 ;
  int blockHistogramSize = blockCells.width * blockCells.height * nbins ; // 得到一个block总共有多少维的HOG数据 2*2*9 = 36 ；
  int rawBlockSize = winSize.width * winSize.height ; 
  // 申请动态内存进行高斯权重的计算
  float ** weights = new float*[winSize.height] ;
  for( int index = 0 ; index < winSize.height ; ++ index )
    weights[index] = new float[winSize.width] ;
  
  float sigma = 2 ;
  float scale = -1.f / (sigma*sigma*2) ;
  // 计算出一个block中各个像素点的高斯权重
  for( int index_rows = 0 ; index_rows < winSize.height ; ++ index_rows )
    for( int index_cols = 0; index_cols  < winSize.width ; ++ index_cols )
      {
	float drow = index_rows - (winSize.height-1)*0.5f ;
	float dcol = index_cols - (winSize.width-1)*0.5f ;
	weights[index_rows][index_cols] = std::exp((drow*drow+dcol*dcol)*scale) ;
      }

  pixData.resize(rawBlockSize) ; 

  // 计算参考的权重表
  // 通过对数据0-15进行相应偏移 映射到 -1 1 之间。使得4个cell的坐标变为 0 1 ;
  // 4个cell的横向中心坐标为3.5 和 11.5 通过 3.5 + 0.5 / 8 - 0.5 = 0 和 11.5+0.5 / 8 - 0.5 = 1 来进行坐标的映射
  
  for( int index_row = 0 ; index_row < winSize.height ; ++ index_row )
    for( int index_col = 0 ; index_col < winSize.width ; ++ index_col )
      {
	PixData * data = NULL ;
	float cellX = (index_col+0.5)/blockSize.width - 0.5f ;
	float cellY = (index_row+0.5)/blockSize.width - 0.5f ;
	int icellX0 = cvFloor(cellX) ;
	int icellY0 = cvFloor(cellY) ;
	int icellX1 = icellX0 + 1 , icellY1 = icellY0 + 1 ;
	cellX -= icellX0 ; // 得到两个到相邻cell的权重
	cellY -= icellY0 ;
	
	/*
	  通过这种映射，可以很好的判断某个pix的所属区域和它对几个cell进行权值贡献。
	  通过对每一个cell进行4分使得16*16的block变成了
	   1  2  3  4
	   5  6  7  8 
	   9 10 11 12
	  13 14 15 16
	  16个块，其中 1，4，13，16只对本身所在的cell贡献权值
	  2 3 5 9 8 12 14 15 只对临近的两个cell贡献权值
	  6 7 10 11 最中间的4个像素块对4个cell都贡献权值
	  (icellX0,icellY0,icellX1,icellY1)
	  (-1,-1,0,0)+(-1,1,0,2)+(1,-1,2,0)+(1,1,2,2) => 1 4 13 16 四个角的4个像素点块，只对所在的cell贡献权值
	  (-1,0,0,1)+(0,-1,1,0)+(1,0,2,1)+(0,1,1,2) => 5 9 2 3 8 12 14 15 四个边界的中间部分对临近的两个cell贡献权值
	  (0,0,1,1) 6 7 10 11 对4个cell都贡献权值
	 */
	if( (unsigned)icellX0 < (unsigned)blockCells.width && (unsigned)icellX1 < (unsigned)blockCells.width )
	  {
	    // icellX0 == 0 
	    if( (unsigned)icellY0 < (unsigned)blockCells.height && (unsigned)icellY1 < (unsigned)blockCells.height )
	      {
		// icellY0 == 0 
		// 贡献4个cell的情况
		data  = &pixData[index_row*winSize.width+index_col];
		data->offset[0] = (icellY0*blockCells.width+icellX0)*nbins ;
		data->weight[0] = (1.f-cellX)*(1.f-cellY) ;
		data->offset[1] = (icellY0*blockCells.width+icellX1)*nbins ;
		data->weight[1] = cellX*(1.f-cellY) ;
		data->offset[2] = (icellY1*blockCells.width+icellX0)*nbins ;
		data->weight[2] = (1.f-cellX)*cellY ;
		data->offset[3] = (icellY1*blockCells.width+icellX1)*nbins ;
		data->weight[3] = cellX*cellY ;
	      }
	    else
	      {
		// 2个cell贡献权值
		data = &pixData[index_row*winSize.width+index_col] ;
		if( (unsigned)icellY0 < (unsigned)blockCells.height )
		  {
		    // Y0 = 1 Y1 =2 
		    icellY1 = icellY0 ; // 1 
		    cellY = 1.f - cellY ;
		  }
		data->offset[0] = (icellY1*blockCells.width+icellX0)*nbins ;
		data->weight[0] = (1.f-cellX)*cellY ;
		data->offset[1] = (icellY1*blockCells.width+icellX1)*nbins ;
		data->weight[1] = cellX*cellY ;
		data->offset[2] = data->offset[3] = 0 ;
		data->weight[2] = data->weight[3] = 0 ;
	      }
	  }
	else
	  {
	    // 调整col的贡献的cell的位置与权值
	    if( (unsigned)icellX0 < (unsigned)blockCells.width )
	      {
		icellX1 = icellX0 ; // 1
		cellX = 1.f - cellX ;
	      }
	    if( (unsigned)icellY0 < (unsigned)blockCells.height && (unsigned)icellY1 < (unsigned)blockCells.height)
	      {
		// 贡献两个cell
		data = &pixData[index_row*winSize.width+index_col] ;
		data->offset[0] = (icellY0*blockCells.width+icellX1)*nbins ;
		data->weight[0] = cellX * (1.f-cellY) ;
		data->offset[1] = (icellY1*blockCells.width+icellX1)*nbins ;
		data->weight[1] = cellX * cellY ;
		data->offset[2] = data->offset[3] = 0 ;
		data->weight[2] = data->weight[3] = 0 ;
	      }
	    else
	      {
		// 贡献一个cell
		data = &pixData[index_row*winSize.width+index_col] ;
		if( (unsigned)icellY0 < (unsigned)blockCells.height )
		  {
		    icellY1 = icellY0 ; // 1 ;
		    cellY = 1.f - cellY ;
		  }
		data->offset[0] = (icellY1*blockCells.width+icellX1)*nbins ;
		data->weight[0] = cellX * cellY ;
		data->offset[1] = data->offset[2] = data->offset[3] = 0 ;
		data->weight[1] = data->weight[2] = data->weight[3] = 0 ;
	      }
	  }
      }
  /*
  for( int index = 0 ; index < count4 ; ++ index )
    {
      std::cout << "index:" << index << " Point:" << pixData[index].location.x << " " << pixData[index].location.y << std::endl ;
      std::cout << "histOfs: 0 " << pixData[index].histOfs[0] << " 1 " << pixData[index].histOfs[1] << " 2 " << pixData[index].histOfs[2] << " 3 " << pixData[index].histOfs[3] << std::endl ;
      std::cout << "histWeights: 0 " << pixData[index].histWeights[0] << " 1 " << pixData[index].histWeights[1] << " 2 " << pixData[index].histWeights[2] << " 3 " << pixData[index].histWeights[3] << std::endl ;
      std::cout << "gradWeight:" << pixData[index].gradWeight << std::endl ;
    }
  */
  for( int index = 0 ; index < blockSize.height ; ++ index )
    delete []weights[index] ;
  delete []weights ;
}
/*
  计算视频中的光流特征
 */
void HistogramOfFlow::getFeaturesFromVideo() 
{
  if( access(featuresVectorFileName.c_str(),F_OK) == 0 )
    {
      INFO2("flow features exist!",featuresVectorFileName) ;
      return ;
    }
  if( access(stipFileName.c_str(),F_OK) == 0 )
    {
      INFO2("stip file exist!",stipFileName) ;
      calculateFeatures() ;
    }
  else
    {
      INFO2("stip file must exist!",stipFileName) ;
      exit(-1) ;
    }
}
/*
  计算flow特征，
  1 2 3 4 5 
  当前求3的flow特征，那么我们需要2，3，4的x，y，t的梯度图，组成一个立方体来进行flow的计算。
  那么要保证整个imageList内至少有5个imagePtr
 */
void HistogramOfFlow::calculateFeatures()
{
  std::ofstream fileOutStream(featuresVectorFileName.c_str()) ;
  std::ifstream fileInStream(stipFileName.c_str()) ;
  if( !fileOutStream.is_open() )
    {
      INFO2("flow features file name",featuresVectorFileName) ;
      ERROR("can't open the flow file") ;
      exit(-1) ;
    }
  if( !fileInStream.is_open() ) 
    {
      INFO2("stip file name",stipFileName) ;
      ERROR("can't open the stip file") ;
      exit(-1) ;
    }
  CvCapture * capture = cvCaptureFromFile(videoFileName.c_str()) ;
  if( capture == NULL )
    {
      INFO2("video file name", videoFileName.c_str()) ;
      ERROR("can't open the video") ;
      exit(-1) ;
    }
  int stipIndex ; // stip 的索引
  pushImage(capture) ; // 压入图像
  int stipPointsLength ; // 探测出的stip的节点数
  std::vector<IplImage*> gradientImageList ; 
  std::vector<IplImage*> subImageList ; 
  cvNamedWindow("source",1) ;
  cvNamedWindow("subImage",1) ;
  cvNamedWindow("velocity",1) ;
  cvMoveWindow("source",30,30);
  cvMoveWindow("subImage",330,30) ;
  cvMoveWindow("velocity",600,30) ;
  while( fileInStream >> stipIndex )
    {
      int beginIndex = stipIndex - 2 ;
      // 查看图像队列中的图像索引是否与需要计算的图像的index相同
      while( imageList.begin()->first < beginIndex )  
	{
	  IplImage * ptr = imageList.begin()->second ;
	  cvReleaseImage(&ptr) ;
	  imageList.pop_front() ;
	  if( imageList.size() < 5 ) 
	    pushImage(capture) ; 
	}
      fileInStream >> stipPointsLength ;
      cv::Point point , pointSource;
      for( int i = 1 ; i <= stipPointsLength ; ++ i )
	{
	  char temp ;
	  fileInStream >> temp >> pointSource.x >> pointSource.y >> temp ;
	  point.x = pointSource.x - (winSize.width>>1) ;
	  point.y = pointSource.y - (winSize.height>>1) ;
	  // 防止数据跳出范围
	  point.x = point.x >= 0 ? point.x : 0 ;
	  point.y = point.y >= 0 ? point.y : 0 ;
	  // 清空返回的数据list
	  while(!gradientImageList.empty())
	    {
	      IplImage * ptr = gradientImageList.back() ;
	      cvReleaseImage(&ptr) ;
	      gradientImageList.pop_back() ;
	    }
	  while(!subImageList.empty())
	    {
	      IplImage * ptr = subImageList.back() ;
	      cvReleaseImage(&ptr) ;
	      subImageList.pop_back() ;
	    }
	  // 判断窗口的位置坐标合法性
	  if( point.x > imageList.front().second->width - winSize.width || point.y > imageList.front().second->height - winSize.height ) 
	    continue ;
	  
	  // 得到子图
	  std::list<std::pair<int,IplImage*> >::iterator iter = imageList.begin() ;
	  for( int index = 0 ; index < 5 ; ++ index )
	    {
	      IplImage *ptr = GetSubImage(iter->second,point,winSize) ; 
	      subImageList.push_back(ptr) ;
	      ++iter ;
	    }
	  // 计算空间及时间上的梯度
	  calculateGradient(subImageList,gradientImageList) ;

	  
	  IplImage* orientImage = cvCreateImage(winSize,IPL_DEPTH_32F,1) ;
	  IplImage* velocityImage = cvCreateImage(winSize,IPL_DEPTH_32F,1) ;
	  
	  calculateOpticFlow(gradientImageList,orientImage,velocityImage) ;

	  cvShowImage("source",(++(++imageList.begin()))->second) ;
	  cvShowImage("subImage",subImageList[2]) ;
	  cvShowImage("velocity",velocityImage) ;
	  cvWaitKey(20) ;
	  std::vector<float> retFeature ;
	  //	  staticFeaturesGaussian(retFeature,orientImage,velocityImage) ;
	  
	  /*
	  char imageName[25];char pointIndex[25] ;
	  itoa(stipIndex,imageName,25) ;
	  itoa(i,pointIndex,25) ;
	  strcat(imageName,pointIndex) ;
	  strcat(imageName,".jpg") ;
	  std::string imageDxFileName = "../PicResult/Dx/" ;
	  std::string imageDyFileName = "../PicResult/Dy/" ;
	  std::string imageWinFileName = "../PicResult/Win/" ;
	  imageDxFileName += imageName ;
	  imageDyFileName += imageName ;
	  imageWinFileName += imageName ;
	  cvSaveImage(imageDxFileName.c_str(),gradientImageList[3]) ;
	  cvSaveImage(imageDyFileName.c_str(),gradientImageList[4]) ;
	  cvSaveImage(imageWinFileName.c_str(),subImageList[2]) ;
	  */
	  //我的方法
	  staticFeaturesMy(retFeature,orientImage,velocityImage,gradientImageList[3],gradientImageList[4]) ;
	  float sum = 0 ;
	  for( int index = 0 ; index < retFeature.size() ; ++ index )
	    sum += retFeature[index] ;
	  for( int index = 0 ; index < retFeature.size() ; ++ index )
	    retFeature[index] *= 1000 / sum ;
	  fileOutStream << "#\t" << stipIndex << "\t[" << pointSource.x << "\t" << pointSource.y << "]\n" ;
	  for( int index = 0 ; index < retFeature.size() ; ++ index )
	    fileOutStream << retFeature[index] << "\t" ;
	  fileOutStream << std::endl ;
	  cvReleaseImage(&orientImage) ;
	  cvReleaseImage(&velocityImage) ;
	}
    }
  cvDestroyAllWindows() ;
  fileInStream.close() ;
  fileOutStream.close() ;
}
/*
  创新点：
  使用梯度来进行统计
  计算梯度大于gradientThreshold时算为统计点 
  计算大于点数A和小于点数B
  大于点的给予权值：3A/(3A+B)
  小于点的给予权值：B/(3A+B)
 */
void HistogramOfFlow::staticFeaturesMy(std::vector<float> &_retFeature, IplImage* _orientImage, IplImage* _velocityImage, IplImage * _xSpaceImage, IplImage * _ySpaceImage)
{
  assert(_velocityImage!=NULL&&_orientImage!=NULL&&_xSpaceImage!=NULL&&_ySpaceImage!=NULL);
  int rows = _orientImage->height ;
  int cols = _orientImage->width ;
  int nChannels = _orientImage->nChannels ;
  int widthStep = _orientImage->widthStep ;
  CvSize blockCount = cvSize(winSize.width/blockSize.width,winSize.height/blockSize.height) ;
  int featureLength = blockCount.width*blockCount.height * 5 ;
  
  _retFeature.resize(featureLength) ;
  for( int index = 0 ; index < featureLength ; ++ index )
    _retFeature[index] = 0 ;
  
  char* _xSpaceData = _xSpaceImage->imageData ;
  char* _ySpaceData = _ySpaceImage->imageData ;
  float* _xSpacePoint = NULL ;
  float* _ySpacePoint = NULL ;
  
  float A = 0 , B = 0 ;

  for( int index_row = 0 ; index_row < rows ; ++ index_row )
    {
      _xSpacePoint = (float*)_xSpaceData ;
      _ySpacePoint = (float*)_ySpaceData ;
      for( int index_col = 0 ; index_col < cols ; ++ index_col )
	{
	  if( hypot(*_xSpacePoint,*_ySpacePoint) >= gradientThreshold ) 
	    ++ A ;
	  else ++ B ;
	  _xSpacePoint += nChannels ;
	  _ySpacePoint += nChannels ;
	}
      _xSpaceData += widthStep ;
      _ySpaceData += widthStep ;
    }
  float weightA = 3*A/(3*A+B) ;
  float weightB = 1 - weightA ;
  //  INFO2("A",A) ;
  //  INFO2("B",B) ;
  char* _orientImageData = _orientImage->imageData ;
  char* _velocityImageData = _velocityImage->imageData ;
  float* _orientImagePoint = NULL ;
  float* _velocityImagePoint = NULL ;
  _xSpaceData = _xSpaceImage->imageData ;
  _ySpaceData = _ySpaceImage->imageData ;

  for( int index_row = 0 ; index_row < rows ; ++ index_row )
    {
      _xSpacePoint = (float*)_xSpaceData ;
      _ySpacePoint = (float*)_ySpaceData ;
      _velocityImagePoint = (float*)_velocityImageData ;
      _orientImagePoint = (float*)_orientImageData ;
      for( int index_col = 0 ; index_col < cols ; ++ index_col )
	{
	  float angle = *_orientImagePoint ;
	  float velocity = *_velocityImagePoint ;
	  struct PixData& data = pixData[index_row*winSize.width+index_col] ;
	  float weightNow = hypot(*_xSpacePoint,*_ySpacePoint) >= gradientThreshold?weightA:weightB ;
	  //weightNow = 1 ;
	  if( velocity < 10 )
	    for( int i = 0 ; i < 4 ; ++ i )
	      _retFeature[data.offset[i]] += velocity*data.weight[i]*weightNow ;
	  else
	    {
	      angle += 45 ;
	      if( angle >= 360 ) angle -= 360 ;
	      int angleFlag = static_cast<int>(angle)/90+1 ;
	      for( int i = 0 ; i < 4 ; ++ i )
		_retFeature[data.offset[i]+angleFlag] += velocity * data.weight[i] * weightNow ;
	    }
	  _xSpacePoint += nChannels ;
	  _ySpacePoint += nChannels ;
	  _orientImagePoint += nChannels ;
	  _velocityImagePoint += nChannels ;
	}
      _xSpaceData += widthStep ;
      _ySpaceData += widthStep ;
      _orientImageData += widthStep ;
      _velocityImageData += widthStep ;
    }
}
/*
  将特征向量分为4个blcok
  每个block有5个特征向量，表示 左，下，右，上，中间不动
 */
void HistogramOfFlow::staticFeaturesGaussian(std::vector<float> &_retFeature, IplImage* _orientImage, IplImage* _velocityImage)
{
  assert( _orientImage != NULL && _velocityImage != NULL ) ;
  CvSize blockCount = cvSize(winSize.width/blockSize.width,winSize.height/blockSize.height) ;
  int featureLength = blockCount.width*blockCount.height * 5 ;

  _retFeature.resize(featureLength) ;
  for( int index = 0 ; index < featureLength ; ++ index )
    _retFeature[index] = 0 ;
  
  uchar* orientImageData = (uchar*)_orientImage->imageData ;
  uchar* velocityImageData = (uchar*)_velocityImage->imageData ;
  float* orientImagePoint = NULL ;
  float* velocityImagePoint = NULL ;

  int rows = _orientImage->height ;
  int cols = _orientImage->width ;
  int nChannels = _orientImage->nChannels ; 
  int widthStep = _orientImage->widthStep ;
  for( int index_row = 0; index_row < rows ; ++ index_row )
    {
      orientImagePoint = (float*)orientImageData ;
      velocityImagePoint = (float*)velocityImageData ;
      
      for( int index_col = 0 ; index_col < cols ; ++ index_col )
	{
	  float angle = *orientImagePoint ;
	  float velocity = *velocityImagePoint ;
	  struct PixData& data = pixData[index_row*winSize.width+index_col] ;
	  if( velocity < 10 ) 
	    for( int i = 0 ; i < 4 ; ++ i )
	      _retFeature[data.offset[i]] += velocity * data.weight[i] ;
	  else
	    {
	      // 偏转45度
	      // 315-44=>0-90     -> 0 左
	      // 45-134=>91-180   -> 1 
	      // 135-224=>181-270 -> 2 
	      // 225-314=>271-360 -> 3 
	      angle += 45 ;
	      if( angle >= 360 ) angle -= 360 ;
	      int angleFlag = static_cast<int>(angle)/90 + 1 ;
	      for( int i = 0 ; i < 4 ; ++ i )
		_retFeature[data.offset[i]+angleFlag] += velocity * data.weight[i] ;
	    }
	  orientImagePoint += nChannels ;
	  velocityImagePoint += nChannels ;
	}
      orientImageData += widthStep ;
      velocityImageData += widthStep ;
    }
}
/*
  通过Lucas-Kanade光流算法计算出Vx,Vy
  并通过Vx,Vy来计算得到方向和速度图像
  IxVx+IyVy=-It
 */
void HistogramOfFlow::calculateOpticFlow(std::vector<IplImage*> &_inImageList,IplImage* _orientImage, IplImage* _velocityImage)
{
  assert(_inImageList.size() == 9 && _orientImage != NULL && _velocityImage != NULL ) ;
  // xSpace Gradient
  gaussian3DSmooth(_inImageList[0],_inImageList[3],_inImageList[6] );
  // ySpace Gradient
  gaussian3DSmooth(_inImageList[1],_inImageList[4],_inImageList[7] );
  // time Gradient
  gaussian3DSmooth(_inImageList[2],_inImageList[5],_inImageList[8] );
  IplImage * xSpaceImage = _inImageList[3] ;
  IplImage * ySpaceImage = _inImageList[4] ;
  IplImage * timeImage = _inImageList[5] ;

  
  float matrix[2][2] ;
  
  uchar* xSpaceImageData = (uchar*)xSpaceImage->imageData ;
  uchar* ySpaceImageData = (uchar*)ySpaceImage->imageData ;
  uchar* timeImageData = (uchar*)timeImage->imageData ;

  float* xSpaceImagePoint = NULL ;
  float* ySpaceImagePoint = NULL ;
  float* timeImagePoint = NULL ;

  uchar* orientImageData = (uchar*)_orientImage->imageData ;
  uchar* velocityImageData = (uchar*)_velocityImage->imageData ;
  float* orientImagePoint = NULL ;
  float* velocityImagePoint = NULL ;

  int rows = xSpaceImage->height ;
  int cols = xSpaceImage->width ;
  int nChannels = xSpaceImage->nChannels ;
  int widthStep = xSpaceImage->widthStep ;
  
  for( int index_row = 0 ; index_row < rows ; ++ index_row )
    {
      xSpaceImagePoint = (float*)xSpaceImageData ;
      ySpaceImagePoint = (float*)ySpaceImageData ;
      timeImagePoint = (float*)timeImageData ;
      
      orientImagePoint = (float*)orientImageData ;
      velocityImagePoint = (float*)velocityImageData ;

      for(int index_col = 0 ; index_col < cols ; ++ index_col )
	{
	  matrix[0][0] = *xSpaceImagePoint * *xSpaceImagePoint ;
	  matrix[0][1] = *xSpaceImagePoint * *ySpaceImagePoint ;
	  matrix[1][0] = matrix[0][1] ;
	  matrix[1][1] = *ySpaceImagePoint * *ySpaceImagePoint ;
	  get2X2InverseMatrix(matrix) ;
	  float velX, velY ;
	  velX = matrix[0][0] * *xSpaceImagePoint * *timeImagePoint + matrix[0][1] * *ySpaceImagePoint * *timeImagePoint; 
	  velY = matrix[1][0] * *xSpaceImagePoint * *timeImagePoint + matrix[1][1] * *ySpaceImagePoint * *timeImagePoint ;
	  velX *= -1.f ;
	  velY *= -1.f ;
	  *velocityImagePoint = hypot(velX,velY) ;
	  *orientImagePoint = atan2(velY,velX) * 180 / PI ;
	  // 调整到0-360;
	  if( *orientImagePoint < 0 ) 
	    *orientImagePoint += 360 ;
	  xSpaceImagePoint += nChannels ;
	  ySpaceImagePoint += nChannels ;
	  timeImagePoint += nChannels ;
	  orientImagePoint += nChannels ;
	  velocityImagePoint += nChannels ;
	}
      xSpaceImageData += widthStep ;
      ySpaceImageData += widthStep ;
      timeImageData += widthStep ;
      orientImageData += widthStep ;
      velocityImageData += widthStep ;
    }
}
/*
  get inverse matirx of a 2 * 2 matrix ;
  算法交换主对角线上的元素，剩下的两个元素求负
  in matrix
  A00 A01
  A10 A11 
  out matrix
  A11 -A01
  -A10 A00 
  {A00*A11 - A01*A10 , A00*A01-A01*A00
  A11*A10-A11*A10, A00*A11-A01*A01}
  归一化 处以dem即可
 */
void HistogramOfFlow::get2X2InverseMatrix(float _matrix[2][2])
{
  assert( _matrix != NULL) ;
  float dem = _matrix[0][0]*_matrix[1][1] - _matrix[0][1]*_matrix[1][0] ;
  if( dem <= 1e-6 && dem >= -1e-6 ) 
    {
      _matrix[0][0] = 0 ;
      _matrix[0][1] = 0 ;
      _matrix[1][0] = 0 ;
      _matrix[1][1] = 0 ;
    }
  else
    {
      std::swap(_matrix[0][0],_matrix[1][1]) ;
      _matrix[0][1] = 0.f - _matrix[0][1] ;
      _matrix[1][0] = 0.f - _matrix[1][0] ;
      _matrix[0][0] /= dem ;
      _matrix[1][1] /= dem ;
      _matrix[0][1] /= dem ;
      _matrix[1][0] /= dem ;
    }
}
/*
  对3幅图像进行立体平滑,平滑结果放在_nowImage ;
 */
void HistogramOfFlow::gaussian3DSmooth(IplImage* _ansImage, IplImage* _nowImage, IplImage * _nextImage)
{
  assert( _ansImage != NULL && _nowImage != NULL && _nextImage != NULL ) ;
  float gaussianKernel[3] = {0.3,0.4,0.3} ;
  
  // 立体平滑
  
  uchar * _imageData[3] ;
  float * _imagePoint[3] ;
  _imageData[0] = (uchar*)_ansImage->imageData ;
  _imageData[1] = (uchar*)_nowImage->imageData ;
  _imageData[2] = (uchar*)_nextImage->imageData ;
  
  int rows = _ansImage->height ;
  int cols = _ansImage->width ;
  int nChannels = _ansImage->nChannels ;
  int widthStep = _ansImage->widthStep ;

  for( int index_rows = 0 ; index_rows < rows ; ++ index_rows )
    {
      _imagePoint[0] = (float*)_imageData[0] ;
      _imagePoint[1] = (float*)_imageData[1] ;
      _imagePoint[2] = (float*)_imageData[2] ;
      for( int index_cols = 0 ; index_cols < cols ; ++ index_cols )
	{
	  *_imagePoint[1] = gaussianKernel[0] * (*_imagePoint[0]) +
	    gaussianKernel[1] * (*_imagePoint[1]) +
	    gaussianKernel[2] * (*_imagePoint[2]) ;
	  _imagePoint[0] += nChannels ;
	  _imagePoint[1] += nChannels ;
	  _imagePoint[2] += nChannels ;

	}
      _imageData[0] += widthStep ;
      _imageData[1] += widthStep ;
      _imageData[2] += widthStep ;
    }
  // 平面平滑
  IplImage* tempImage = cvCreateImage(cvGetSize(_nowImage),IPL_DEPTH_32F,1) ;
  if( tempImage == NULL )
    {
      ERROR("can't create image!");
      exit(-1) ;
    }
  float ** rowPtr = new float*[_nowImage->height] ;
  if( rowPtr == NULL )
    {
      ERROR("can't new a array") ;
      exit(-1) ;
    }
  for( int index = 0 ; index < _nowImage->height ; ++ index )
    rowPtr[index] = (float*)(_nowImage->imageData+_nowImage->widthStep*index) ;
  
  uchar * tempImageData = (uchar*)tempImage->imageData ;
  float * tempImagePoint = NULL ; 
  
  for( int index_row = 0 ; index_row < rows ; ++ index_row )
    {
      tempImagePoint = (float*)tempImageData ;
      for( int index_col = 0 ; index_col < cols ; ++ index_col )
	{
	  int up_row = index_row - 1 ;
	  int left_col = index_col - 1 ;

	  if( up_row == -1 ) up_row = 0 ;
	  if( left_col == -1 ) left_col = 0 ;
	  if( up_row == rows - 2 ) -- up_row ;
	  if( left_col == cols - 2 ) -- left_col ;

	  float sum = 0 ;
	  for( int row = 0 ; row < 3 ; ++ row )
	    for( int col = 0 ; col < 3 ; ++ col )
	      sum += rowPtr[up_row+row][left_col+col]*gaussianKernel[row]*gaussianKernel[col] ;
	  *tempImagePoint = sum ;
	  tempImagePoint += tempImage->nChannels ;
	}
      tempImageData += tempImage->widthStep ;
    }
  cvCopyImage(tempImage,_nowImage) ;
  delete [] rowPtr ;
  cvReleaseImage(&tempImage) ;
}
/*
  计算梯度
  _inImageList 存储需要计算的5幅图像 1 2 3 4 5
  _outImageList 返回结果 
  x梯度图，y梯度图，t梯度图三个一组，共3组9幅图像
*/
void HistogramOfFlow::calculateGradient(std::vector<IplImage*> &_inImageList,std::vector<IplImage*> &_outImageList)
{
  assert( _inImageList.size() == 5 || _outImageList.size() == 0 ) ;
  IplImage* imagePtrArray[5] ;
  std::vector<IplImage*>::iterator iter = _inImageList.begin() ;
  
  for( int i = 0 ; i < 5 ; ++ i )
    {
      imagePtrArray[i] = *iter ;
      ++ iter ;
    }
  for( int i = 1 ; i < 4 ; ++ i )
    {
      IplImage * xSpaceImage = cvCreateImage(cvGetSize(imagePtrArray[i]),IPL_DEPTH_32F,1) ;
      IplImage * ySpaceImage = cvCreateImage(cvGetSize(imagePtrArray[i]),IPL_DEPTH_32F,1) ;
      IplImage * timeImage = cvCreateImage(cvGetSize(imagePtrArray[i]),IPL_DEPTH_32F,1) ;
      // 计算空间上的梯度
      calculateSpaceGradient(imagePtrArray[i],xSpaceImage,ySpaceImage) ;
      // 计算时间上的梯度
      calculateTimeGradient(imagePtrArray[i-1],imagePtrArray[i+1],timeImage) ;
      _outImageList.push_back(xSpaceImage) ;
      _outImageList.push_back(ySpaceImage) ;
      _outImageList.push_back(timeImage) ;
    }
  assert(_outImageList.size() == 9 ) ;
}
void HistogramOfFlow::calculateSpaceGradient(IplImage* _src, IplImage* _xSpaceDst, IplImage* _ySpaceDst)
{

  assert(_src!=NULL&&_xSpaceDst!=NULL&&_ySpaceDst!=NULL) ;
  cvZero(_xSpaceDst) ;
  cvZero(_ySpaceDst) ;
  uchar** rowPtr = new uchar*[_src->height] ;
  for( int index_row = 0 ; index_row < _src->height ; ++ index_row )
    rowPtr[index_row] = (uchar*)(_src->imageData + index_row * _src->widthStep) ;
  for( int index_row = 1 ; index_row < _src->height - 1 ; ++ index_row )
    for( int index_col = 1 ; index_col < _src->width - 1 ; ++ index_col )
      {
	float gradientXValue = 0 , gradientYValue = 0 ;
	uchar* ptrelem[3] ;
	for( int index = 0 ; index < 3 ; ++ index )
	  ptrelem[index] = rowPtr[index_row-1+index] + index_col - 1 ;
	gradientXValue = -1.f*ptrelem[0][0] + 1.f*ptrelem[0][2] - 2.f*ptrelem[1][0] + 2.f*ptrelem[1][2] - 1.f*ptrelem[2][0] + 1.f*ptrelem[2][2] ;
	gradientXValue *= 0.25f ;
	gradientYValue = -1.f*ptrelem[0][0] - 2.f*ptrelem[0][1] - 1.f*ptrelem[0][2] + 1.f*ptrelem[2][0] + 2.f*ptrelem[2][1] + 1.f*ptrelem[2][2] ;
	gradientYValue *= 0.25f ;
	*((float*)(_xSpaceDst->imageData+index_row*_xSpaceDst->widthStep)+index_col) = gradientXValue ;
	*((float*)(_ySpaceDst->imageData+index_row*_ySpaceDst->widthStep)+index_col) = gradientYValue ;
      }
  delete rowPtr ;
}
void HistogramOfFlow::calculateTimeGradient(IplImage* _ansImage, IplImage* _nextImage, IplImage* _retImage) 
{
  assert(_ansImage!=NULL||_nextImage!=NULL||_retImage!=NULL) ;
  int rows = _ansImage->height ;
  int cols = _ansImage->width ;
  int nChannels = _ansImage->nChannels ;
  int widthStep = _ansImage->widthStep ;
  
  uchar* _ansImageData = (uchar*)(_ansImage->imageData) ;
  uchar* _ansImagePoint = NULL ;
  uchar* _nextImageData = (uchar*)(_nextImage->imageData) ;
  uchar* _nextImagePoint = NULL ;
  uchar* _retImageData = (uchar*)(_retImage->imageData) ;
  float* _retImagePoint = NULL ;

  for( int index_row = 0 ; index_row < rows ; ++ index_row )
    {
      _ansImagePoint = _ansImageData ;
      _nextImagePoint = _nextImageData ;
      _retImagePoint = (float*)_retImageData ;
      for( int index_col = 0 ; index_col < cols ; ++ index_col )
	{
	  *(_retImagePoint) = float(*_ansImagePoint) - float(*_nextImagePoint) ;
	  _ansImagePoint += nChannels ;
	  _nextImagePoint += nChannels ;
	  _retImagePoint += nChannels ;
	}
      _ansImageData += widthStep ;
      _nextImageData += widthStep ;
      _retImageData += _retImage->widthStep ;
    }
}
/* 得到子图
 _src 输入的图像
 _point 子图的左上起点
 _size 子图的尺寸
 返回子图的指针
*/ 
IplImage* HistogramOfFlow::GetSubImage(IplImage* _src, cv::Point _point, CvSize _size)
{
  if( _src == NULL )
    {
      ERROR("pointer of image is NULL!") ;
      exit(-1) ;
    }
  // 初始化ROI区域
  CvRect roiRect = cvRect(_point.x,_point.y,_size.width,_size.height) ;
  // 申请子图内存
  IplImage* _dst = cvCreateImage(_size, IPL_DEPTH_8U, _src->nChannels) ;
  // 设置源图像的ROI
  cvSetImageROI(_src,roiRect) ;
  // 幅值
  cvCopy(_src,_dst) ;
  // 重新恢复原图的ROI
  cvResetImageROI(_src) ;
  return _dst ;
}
// 一次push 10帧进入队列
unsigned HistogramOfFlow::pushImage(CvCapture* _capture)
{
  if( _capture == NULL )
    {
      ERROR("capture is NULL!") ;
      exit(-1) ;
    }
  int index ; 
  if( imageList.empty() ) 
    {
      index = 0 ;
    }
  else
    {
      index = imageList.back().first + 1 ;
    }
  int i ; 
  for( i = 0 ; i < 10 ; ++ i )
    {
      IplImage * imagePtr = cvQueryFrame(_capture) ;
      if( imagePtr == NULL ) break ;
      IplImage * copyImage ;
      if( imagePtr->nChannels == 1 )
	copyImage = cvCloneImage(imagePtr) ;
      else
	{
	  IplImage * grayImage = cvCreateImage(cvGetSize(imagePtr),IPL_DEPTH_8U,1) ;
	  cvCvtColor(imagePtr,grayImage,CV_BGR2GRAY) ;
	  copyImage = grayImage ;
	}
      imageList.push_back(std::make_pair(index++,copyImage)) ;
    }
  return i ;
}

